#include <string.h>
#include <stdio.h>
#include <stdlib.h>
/* IAR includes. __clz */
// #include "arm_acle.h"

#include "kfifo.h"

#define USE_LWMEM_MEM_MANG 0

#define min(a, b)                (((a) < (b)) ? (a) : (b))

#if USE_LWMEM_MEM_MANG == 1
  #include "lwmem/lwmem.h"
  #define _k_malloc(x) lwmem_malloc(x)
  #define _k_free(x)  lwmem_free(x)
#else
  #define _k_malloc(x)  malloc(x)
  #define _k_free(x)    free(x)
#endif

//TODO fifo溢出警告

// static unsigned int roundup_pow_of_two(unsigned int date_roundup_pow_of_two)
// {
//   return (1UL << (32UL - (unsigned int) __clz((date_roundup_pow_of_two))));
// }

static unsigned int roundup_pow_of_two(unsigned int number)
{
  number--;
  number |= number >> 1;
  number |= number >> 2;
  number |= number >> 4;
  number |= number >> 8;
  number |= number >> 16;

  return number + 1;
}

/**
 * @brief Dynamic create kfifo.
 */
struct KFIFO *kfifo_alloc(unsigned int size)
{
  unsigned char *buffer;
  struct KFIFO *ret;
  ret = (struct KFIFO *) _k_malloc(sizeof(struct KFIFO));
  /*
   * round up to the next power of 2, since our 'let the indices
   * wrap' tachnique works only in this case.
   */
  if(size & (size - 1))
  {
    size = roundup_pow_of_two(size);
  }
  buffer = (unsigned char *) _k_malloc(size);
  memset(buffer, 0x00, size);
  if(!buffer)
    return 0UL;
  ret->buffer = buffer;
  ret->size  = size;
  ret->in  = 0;
  ret->out = 0;
  ret->is_full = 0;
  if(!ret)
    _k_free(buffer);
  return ret;
}

/**
 * @brief Dynamic delete kfifo.
 *
 * @param fifo
 */
void kfifo_free(struct KFIFO *fifo)
{
  _k_free(fifo->buffer);
  _k_free(fifo);
}

/**
 * @brief Init fifo.
 * @note Use in static define.
 *
 * @param fifo
 */
void kfifo_init(struct KFIFO *fifo)
{
  fifo->in = 0;
  fifo->out = 0;
  fifo->is_full = 0;
}

/**
 * @brief Get unused fifo length.
 *
 * @param fifo
 * @return unsigned int
 */
static inline unsigned int kfifo_avail(struct KFIFO *fifo)
{
  return fifo->size - (fifo->in - fifo->out);
}

/**
 * @brief  This function copies at most @len bytes from the @from buffer into
 *         the FIFO depending on the free space, and returns the number of
 *         bytes copied.
 *
 * @param fifo
 * @param buffer
 * @param len
 * @return unsigned int
 */
unsigned int kfifo_put(struct KFIFO *fifo, unsigned char *buffer, unsigned int len)
{
  unsigned int L;
  /* get unused fifo length */
  len = min(len, kfifo_avail(fifo));
  /* copy data to fifo  */
  L = min(len, fifo->size - (fifo->in & (fifo->size - 1)));
  memcpy(fifo->buffer + (fifo->in & (fifo->size - 1)), buffer, L);
  /* then put the rest (if any) at the beginning of the buffer */
  memcpy(fifo->buffer, buffer + L, len - L);
  /* update fifo->in */
  fifo->in += len;
  if(fifo->in == fifo->out)
    fifo->is_full = 1;
  return len;
}

/**
 * @brief
 *
 * @param fifo
 * @param buffer
 * @param len
 * @return unsigned int
 */
unsigned int kfifo_get(struct KFIFO *fifo, unsigned char *buffer, unsigned int len)
{
  unsigned int L;
  unsigned int tmp_in = fifo->in;
  /* get used fifo length */
  len = min(len, tmp_in - fifo->out);
  /* copy data from fifo */
  L = min(len, fifo->size - (fifo->out & (fifo->size - 1)));
  memcpy(buffer, fifo->buffer + (fifo->out & (fifo->size - 1)), L);
  /* then get the rest (if any) from the beginning of the buffer */
  memcpy(buffer + L, fifo->buffer, len - L);
  fifo->out += len;
  /* len is not equal 0, fifo can not full. */
  if(len != 0)
    fifo->is_full = 0;
  return len;
}

/**
 * @brief Get fifo used length.
 *
 * @param fifo
 * @return unsigned int
 */
unsigned int kfifo_len(struct KFIFO *fifo)
{
  if(kfifo_is_full(fifo))
    return fifo->size;
  return (fifo->in - fifo->out) & (fifo->size - 1);
}

unsigned int kfifo_is_empty(struct KFIFO *fifo)
{
  if(kfifo_is_full(fifo))
    return 0;
  return (fifo->in == fifo->out);
}

unsigned int kfifo_is_full(struct KFIFO *fifo)
{
  return fifo->is_full;
}

/**
 * @brief Read char but not remove buffer data.
 *
 * @param fifo
 * @param idx
 * @return char
 */
char kfifo_peek(struct KFIFO *fifo, unsigned int idx)
{
  unsigned int offset = (fifo->out + idx) & (fifo->size - 1);
  return *(fifo->buffer + offset);
}

/**
 * @brief Read pdata buf not remove buffer data.
 *
 * @param fifo
 * @param buffer
 * @param len
 * @return unsigned int
 */
unsigned int kfifo_out_peek(struct KFIFO *fifo, unsigned char *buffer, unsigned int len)
{
  unsigned int L;
  unsigned int tmp = fifo->in;
  len = min(len, tmp - fifo->out);
  /* first get the data from fifo->out until the end of the buffer */
  L = min(len, fifo->size - (fifo->out & (fifo->size - 1)));
  memcpy(buffer, fifo->buffer + (fifo->out & (fifo->size - 1)), L);
  /* then get the rest (if any) from the beginning of the buffer */
  memcpy(buffer + L, fifo->buffer, len - L);
  return len;
}

/**
 * @brief Remove fifo.
 *
 * @param fifo
 * @param len
 */
void kfifo_throw(struct KFIFO *fifo, unsigned int len)
{
  //TODO错误情况判断
  fifo->out += len;
}
